#include "data_adapter.h"

#include <filesystem>
#include <iostream>

#include "common/basic_settings.h"
#include "common/file_helper.h"
#include "common/utility.h"
#include "preprocess/image_process.h"

namespace autodrive {
namespace perception {

DataAdapter::DataAdapter()
    : is_init_{false},
      is_run_{false},
      cmd_stop_{false},
      calib_data_set_ptr_{nullptr},
      image_source_buffer_ptr_{nullptr},
      point_cloud_buffer_ptr_{nullptr} {}

DataAdapter::~DataAdapter() { Stop(); }

DataAdapter& DataAdapter::GetInstance() noexcept {
  static DataAdapter instance;
  return instance;
}

DataAdapter::DataAdapterSettings DataAdapter::gen_default() const noexcept {
  return DataAdapterSettings{
      .data_manager_settings{
          .data_scene_paths{"/home/datasets/nuscenes_formated/scene-0092",
                            "/home/datasets/nuscenes_formated/scene-0017"},
          .load_data_frame_settings{
              .lidars{"inno_frl_0_raw"},
              .cameras{"front", "back", "left_back", "left_front", "right_back",
                       "right_front"},
              .radars{}}},
      .image_process_parameters_set{
          ImageProcessParameters{
              .id = "CAM_FRONT_LEFT",
              .crop_area{.x = 20, .y = 100, .height = 680, .width = 1870},
              .resize_size{.height = 256, .width = 704},
              .order = 0,
              .norm_mean{0, 0, 0},
              .norm_std{0, 0, 0}},
          ImageProcessParameters{
              .id = "CAM_FRONT",
              .crop_area{.x = 600, .y = 1000, .height = 950, .width = 2612},
              .resize_size{.height = 256, .width = 704},
              .order = 1,
              .norm_mean{0, 0, 0},
              .norm_std{0, 0, 0}},
          ImageProcessParameters{
              .id = "CAM_FRONT_RIGHT",
              .crop_area{.x = 20, .y = 100, .height = 680, .width = 1870},
              .resize_size{.height = 256, .width = 704},
              .order = 2,
              .norm_mean{0, 0, 0},
              .norm_std{0, 0, 0}},
          ImageProcessParameters{
              .id = "CAM_BACK_LEFT",
              .crop_area{.x = 20, .y = 100, .height = 680, .width = 1870},
              .resize_size{.height = 256, .width = 704},
              .order = 3,
              .norm_mean{0, 0, 0},
              .norm_std{0, 0, 0}},
          ImageProcessParameters{
              .id = "CAM_BACK",
              .crop_area{.x = 20, .y = 100, .height = 680, .width = 1870},
              .resize_size{.height = 256, .width = 704},
              .order = 4,
              .norm_mean{0, 0, 0},
              .norm_std{0, 0, 0}},
          ImageProcessParameters{
              .id = "CAM_BACK_RIGHT",
              .crop_area{.x = 20, .y = 100, .height = 680, .width = 1870},
              .resize_size{.height = 256, .width = 704},
              .order = 5,
              .norm_mean{0, 0, 0},
              .norm_std{0, 0, 0}}},
      .point_cloud_process_parameters{.lidar_names{"inno_frl_0_raw"}}};
}

void DataAdapter::Start() noexcept {
  auto runFunc{[&]() { task_thread_ = std::thread{main_task, this}; }};
  std::call_once(run_flag_, runFunc);
  is_run_ = true;
}

void DataAdapter::Stop() noexcept {
  std::lock_guard<std::mutex> l(lock_);
  if (IsRun()) {
    cmd_stop_ = true;
    if (task_thread_.joinable()) {
      task_thread_.join();
    }
    auto& dataManager{DataManager::GetInstance()};
    dataManager.Close();
    is_run_ = false;
  }
}

void DataAdapter::Init() noexcept(false) {
  auto initFunc{[this]() {
    std::string const& configFileName{"DataAdapterSettings.json"};
    auto const& configFile{std::filesystem::path(sConfigRootPath) /
                           std::filesystem::path(configFileName)};
    if (!std::filesystem::exists(sConfigRootPath)) {
      if (!std::filesystem::create_directories(sConfigRootPath)) {
        throw std::runtime_error("create directory " + sConfigRootPath +
                                 " fail");
      }
    }

    if (!std::filesystem::exists(configFile)) {
      // use default settings
      auto const& defaultSettings{gen_default()};
      auto const& sjSettings{serialize_json(defaultSettings)};
      write_json(configFile.string(), sjSettings);
    }

    auto const& sSettings{read_text(configFile)};
    settings_ = json_deserialize<DataAdapterSettings>(sSettings);
  }};

  std::call_once(init_flag_, initFunc);
  is_init_ = true;
}

void DataAdapter::update_calib(
    DataScene::CalibDataArrayPtr<DataScene::MatrixData> src_data_ptr,
    std::string const& id, CalibDataSetPtr dst_data_ptr) noexcept(false) {
  if (nullptr == src_data_ptr) {
    throw std::runtime_error("src_data_ptr is nullptr");
  }

  if (nullptr == dst_data_ptr) {
    throw std::runtime_error("dst_data_ptr is nullptr");
  }

  dst_data_ptr->data_set.clear();
  for (auto const& calibData : *src_data_ptr) {
    CalibData cData;
    cData.name = calibData.name;
    cData.data = calibData.raw_data;
    cData.is_valid = calibData.is_valid;
    dst_data_ptr->data_set.add(cData);
  }

  dst_data_ptr->id = id;
  ++dst_data_ptr->update_flag;
}

void DataAdapter::gen_image_source(
    DataScene::CameraDataArray const& camera_data_set,
    std::vector<ImageProcessParameters> const& paras,
    ImageSource<float>& image_source) noexcept(false) {
  image_source.images.clear();
  auto imageNumber{paras.size()};
  image_source.images.resize(imageNumber);
  for (auto const& para : paras) {
    bool isFind{false};
    for (auto const& cameraData : camera_data_set) {
      if (para.id == cameraData.name) {
        crop_resize(cameraData.image,
                    {para.crop_area.x, para.crop_area.y, para.crop_area.width,
                     para.crop_area.height},
                    {para.resize_size.width, para.resize_size.height},
                    image_source.images.at(para.order));
        isFind = true;
        break;
      }
    }
    if (!isFind) {
      throw std::runtime_error("no " + para.id + " found in camera data set");
    }
  }
}

void DataAdapter::gen_point_cloud_source(
    DataScene::LidarDataArray const& lidar_data_set,
    PointCloudProcessParameters const& paras,
    PointCloudSource& pcl_source) noexcept(false) {
  pcl_source.points.clear();
  if (paras.lidar_names.size() <= 0) {
    return;
  }
  bool isFind{false};
  auto const& lidarName{paras.lidar_names.at(0)};

  for (auto const& lidarData : lidar_data_set) {
    if (lidarData.name == lidarName) {
      for (auto const& p : lidarData.point_cloud) {
        pcl_source.points.add(
            {p.x, p.y, p.z, p.intensity, p.timestamp, p.ring});
      }
      isFind = true;
      break;
    }
  }

  if (!isFind) {
    throw std::runtime_error("no " + lidarName + " found in lidar data set");
  }
}

void DataAdapter::main_task(DataAdapter* dataAdapterPtr) noexcept {
  try {
    if (nullptr == dataAdapterPtr) {
      throw std::runtime_error("dataAdapterPtr is nullptr");
    }

    auto const& settings{dataAdapterPtr->Settings()};

    if (nullptr == dataAdapterPtr->image_source_buffer_ptr_) {
      dataAdapterPtr->image_source_buffer_ptr_ =
          std::make_shared<ImageSourceBuffer>();
    }

    if (nullptr == dataAdapterPtr->point_cloud_buffer_ptr_) {
      dataAdapterPtr->point_cloud_buffer_ptr_ =
          std::make_shared<PointCloudBuffer>();
    }

    // 1. init data manager
    auto& dataManager{DataManager::GetInstance()};
    dataManager.Init(settings.data_manager_settings);

    dataManager.Load();

    dataAdapterPtr->msleep(50);
    // 2. feed data
    std::int32_t data_count{0};
    std::uint64_t formerTimestamp{0};
    std::filesystem::path formerScenePath{""};

    while (true) {
      // command to stop task
      if (dataAdapterPtr->cmd_stop_) {
        std::cout << "[" << to_yellow("Warr") << "]"
                  << " DataAdapter command to stop ..." << std::endl;
        break;
      }
      try {
        auto const& dataFramePair{dataManager.LastestDataFrame()};
        auto const& dataFrame{dataFramePair.first};
        auto const& dataScene{dataFramePair.second};

        if (dataFrame.timestamp == formerTimestamp) {
          dataAdapterPtr->msleep(10);
          continue;
        } else {
          formerTimestamp = dataFrame.timestamp;
        }

        // image
        {
          auto imageSourcePtr{
              dataAdapterPtr->image_source_buffer_ptr_->GetNext()};

          dataAdapterPtr->gen_image_source(
              dataFrame.camera_data,
              dataAdapterPtr->Settings().image_process_parameters_set,
              *imageSourcePtr);
          imageSourcePtr->timestamp = dataFrame.timestamp;

          dataAdapterPtr->image_source_buffer_ptr_->Append();
        }

        // point cloud
        {
          auto pclSourcePtr{dataAdapterPtr->point_cloud_buffer_ptr_->GetNext()};

          dataAdapterPtr->gen_point_cloud_source(
              dataFrame.lidar_data,
              dataAdapterPtr->Settings().point_cloud_process_parameters,
              *pclSourcePtr);
          pclSourcePtr->timestamp = dataFrame.timestamp;

          dataAdapterPtr->point_cloud_buffer_ptr_->Append();
        }

        // calib
        {
          if (dataAdapterPtr->calib_data_set_ptr_ == nullptr) {
            dataAdapterPtr->calib_data_set_ptr_ =
                std::make_shared<CalibDataSet>();
            dataAdapterPtr->calib_data_set_ptr_->update_flag = -1;
          }

          if (dataScene.Path() != formerScenePath) {
            formerScenePath = dataScene.Path();
            std::cout << "[" << to_green("Info") << "] "
                      << "calibration swich: " << formerScenePath << std::endl;

            auto const& calibDataArrayPtr{dataScene.CalibData()};
            dataAdapterPtr->update_calib(calibDataArrayPtr,
                                         formerScenePath.string(),
                                         dataAdapterPtr->calib_data_set_ptr_);
          }
        }

      } catch (const std::exception& e) {
        std::cout << to_red("[Error]") << "DataAdapter feed data error, "
                  << e.what() << std::endl;
        dataAdapterPtr->msleep(10);
      }
    }

  } catch (const std::exception& e) {
    std::cout << to_red("[Error]") << "DataAdapter run error, " << e.what()
              << std::endl;
  } catch (...) {
    std::cout << to_red("[Error]") << "DataAdapter run error, "
              << "unexpected" << std::endl;
  }
}

}  // namespace perception
}  // namespace autodrive